/*
 * @Author: fengqiang111 fengqiang@benewake.com
 * @Date: 2022-11-11 12:37:44
 * @LastEditors: fengqiang111 fengqiang@benewake.com
 * @LastEditTime: 2023-01-31 22:58:34
 * @FilePath: /src/jade-dragon/include/services/service.hpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#ifndef __SERVICES_HPP_
#define __SERVICES_HPP_
#include <string>
#include <deque>
#include <mutex>
#include <vector>
#include <map>
#include <thread>
#include <iostream>

#include "ptz_control/absolute_move.h"
#include "ptz_control/continuous_move.h"
#include "ptz_control/goto_preset.h"
#include "ptz_control/stop_move.h"

namespace ptz
{

#define ABSOLUTE_MOVE_SRV "/absolute_move_srv"
#define CONTINUOUS_MOVE_SRV "/continuous_move_srv"
#define GOTO_PRESET_SRV "/goto_preset_srv"
#define STOP_MOVE_SRV "/stop_move_srv"

    class Service
    {
    public:
        bool gotoPresetService( int iPreset )
        {
            bool ret = ros::service::waitForService(GOTO_PRESET_SRV, ros::Duration(1));
            if (!ret)
            {
                return false;
            }
            ros::ServiceClient client = nh_.serviceClient<ptz_control::goto_preset>(GOTO_PRESET_SRV);
            ptz_control::goto_preset srv;
            srv.request.preset_id = iPreset;
            client.call(srv);
            return srv.response.success;
        }

        bool absoluteMoveService( float pan, float titl, float zoom )
        {
            bool ret = ros::service::waitForService(ABSOLUTE_MOVE_SRV, ros::Duration(1));
            if (!ret)
            {
                return false;
            }
            ros::ServiceClient client = nh_.serviceClient<ptz_control::absolute_move>(ABSOLUTE_MOVE_SRV);
            ptz_control::absolute_move srv;
            srv.request.pan = pan;
            srv.request.titl = titl;
            srv.request.zoom = zoom;
            client.call(srv);
            return srv.response.success;
        }

        bool continuousMoveService( int cmd, float speed )
        {
            bool ret = ros::service::waitForService(CONTINUOUS_MOVE_SRV, ros::Duration(1));
            if (!ret)
            {
                return false;
            }
            ros::ServiceClient client = nh_.serviceClient<ptz_control::continuous_move>(CONTINUOUS_MOVE_SRV);
            ptz_control::continuous_move srv;
            srv.request.cmd = cmd;
            srv.request.speed = speed;
            client.call(srv);
            return srv.response.success;
        }

        bool stopMoveService()
        {
            bool ret = ros::service::waitForService(STOP_MOVE_SRV, ros::Duration(1));
            if (!ret)
            {
                return false;
            }
            ros::ServiceClient client = nh_.serviceClient<ptz_control::stop_move>(STOP_MOVE_SRV);
            ptz_control::stop_move srv;
            client.call(srv);
            return srv.response.success;
        }

        ros::NodeHandle nh_;
    };
}

#endif
